#include "defaults.h"
#include "nurbs.h"
#include "utils.h"
#include "optimizer.h"
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/filesystem/operations.hpp>
#include "wrapper/superluwrapper.h"
#include "wrapper/gslwrapper.h"
#include "timing/time.h"

#ifdef _OPENMP
#include <omp.h>
#endif

namespace Optimizer {
			
nfloat EnergyEvaluator::operator()(const nfloat& t) const {
	const size_t n = mesh->SizeNodes();
#ifdef _OPENMP
#pragma omp parallel for
#endif
	for(int i = 0; i < n; i++) {
		Node* v = mesh->GetNode(i);
		const Vec3n& P1 = v->Get<Vec3n>(attribute1);
		Vec3n& P2 = v->Get<Vec3n>(attribute2);
		if(v->Get<size_t>("mark") == 0) {
			const size_t& id = v->Id();
			const Vec3n offset = math::V3n(g(3 * id + 0), g(3 * id + 1), g(3 * id + 2));
			P2 = P1 + t * offset;
		} else
			P2 = P1;
	}

	return eval->f(attribute2);
}

nfloat EnergyLocalEvaluator::operator()(const nfloat& t) const {
	const Vec3n& P1 = v->Get<Vec3n>(attribute1);
	Vec3n& P2 = v->Get<Vec3n>(attribute2);
	P2 = P1 + t * g;
	return eval->f(v, attribute2);
}

static std::pair<nfloat, nfloat> brent(nfloat xLeft, nfloat xRight, nfloat xCenter, const FunctionEvaluator& f, nfloat tol) {
#define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d)
#define SIGN(a,b) ((b) >= 0.0 ? std::abs(a) : -std::abs(a))
	nfloat ax = xLeft;
	nfloat bx = xCenter;
	nfloat cx = xRight;

	const nfloat ITMAX = 100;
	const nfloat CGOLD = 0.3819660;
	const nfloat ZEPS = 1.0e-10;
	int iter;
	nfloat a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm;
	nfloat e=0.0;
	a=(ax < cx ? ax : cx);
	b=(ax > cx ? ax : cx);
	x=w=v=bx;
	fw=fv=fx= f(x);
	for(iter=1;iter<=ITMAX;iter++) {
		xm=0.5*(a+b);
		tol2=2.0*(tol1=tol*std::abs(x)+ZEPS);
		if(std::abs(x-xm) <= (tol2-0.5*(b-a))) {
			return std::pair<nfloat, nfloat>(x, fx);
		}
		if(std::abs(e) > tol1) {
			r=(x-w)*(fx-fv);
			q=(x-v)*(fx-fw);
			p=(x-v)*q-(x-w)*r;
			q=2.0*(q-r);
			if(q > 0.0)
				p = -p;
			q=std::abs(q);
			etemp=e;
			e=d;
			if(std::abs(p) >= std::abs(0.5*q*etemp) || p <= q*(a-x) || p >= q*(b-x))
				d=CGOLD*(e=(x >= xm ? a-x : b-x));
			else {
				d=p/q;
				u=x+d;
				if (u-a < tol2 || b-u < tol2)
				d=SIGN(tol1,xm-x);
			} 
		} else {
			d=CGOLD*(e=(x >= xm ? a-x : b-x));
		}
		u=(std::abs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
		fu= f(u);
		//This is the one function evaluation per iteration.
		if (fu <= fx) {
			if (u >= x) a=x; else b=x;
			SHFT(v,w,x,u);
			SHFT(fv,fw,fx,fu);
		} else {
			if (u < x) a=u; else b=u;
			if (fu <= fw || w == x) {
				v=w;
				w=u;
				fv=fw;
				fw=fu;
			} else if (fu <= fv || v == x || v == w) {
				v=u;
				fv=fu;
			}
		}
	}

	std::cerr << "error: Too many iterations in brent";
	return std::pair<nfloat, nfloat>(x, fx); //Never get here.
#undef SHFT
#undef SIGN
}

std::pair<nfloat, nfloat> StepSizeEstimator::operator()(const nfloat t, const Vector& g) {
	eeval.SetGradient(g);
	return std::pair<nfloat, nfloat>(t, eeval(t));
}

std::pair<nfloat, nfloat> StepSizeLocalEstimator::operator()(const nfloat t, const Vec3n& g, Node* v) {
	eeval.SetGradient(g);
	eeval.SetVertex(v);
	return std::pair<nfloat, nfloat>(t, eeval(t));
}

std::pair<nfloat, nfloat> LineSearchStepSizeEstimator::operator()(const nfloat t, const Vector& g) {
	eeval.SetGradient(g);

	const size_t maxit = 10;
// 	const size_t maxit = 0;
	size_t i = 0;
	std::pair<nfloat, nfloat> p;
	nfloat t0 = 0.5 * t;
	do {
		t0 *= 2.0;
		nfloat tolerance = t0 * 0.0001;
		const nfloat xLeft = 0.0;
		const nfloat xRight = t0;
		const nfloat xCenter = 0.5 * (xLeft + xRight);

		p = brent(xLeft, xRight, xCenter, eeval, tolerance);

		i++;
	} while(i < maxit && std::abs(t0 - p.first) < 0.001);

	return p;
}

std::pair<nfloat, nfloat> LineSearchStepSizeLocalEstimator::operator()(const nfloat t, const Vec3n& g, Node* v) {
	eeval.SetGradient(g);
	eeval.SetVertex(v);

	const size_t maxit = 0;
	size_t i = 0;
	std::pair<nfloat, nfloat> p;
	nfloat t0 = 0.5 * t;
	do {
		t0 *= 2.0;
		nfloat tolerance = t0 * 0.0001;
		const nfloat xLeft = 0.0;
		const nfloat xRight = t0;
		const nfloat xCenter = 0.5 * (xLeft + xRight);

		p = brent(xLeft, xRight, xCenter, eeval, tolerance);

		i++;
	} while(i < maxit && std::abs(t0 - p.first) < 0.001);

	return p;
}
		
static void GlobalAssembly(const std::vector<size_t>& id, const Vector& fLocal, const size_t& n, Vector& fGlobal) {
	for(size_t i = 0; i < n; i++)
		for(size_t j = 0; j < 3; j++)
			fGlobal(3 * id[i] + j) += fLocal(3 * i + j);
}

static void GlobalAssembly(const std::vector<size_t>& id, const Matrix& ALocal, const size_t& n, std::map< std::pair<size_t, size_t>, nfloat >& AGlobal) {
	for(size_t i = 0; i < n; i++)
		for(size_t j = 0; j < n; j++)
			for(size_t k = 0; k < 3; k++)
				for(size_t l = 0; l < 3; l++)
					AGlobal[std::pair<size_t, size_t>(3 * id[i] + k, 3 * id[j] + l)] += ALocal(3 * i + k, 3 * j + l); 
}

/////////////////////////////////////////////////////////////////////////////////////
nfloat NurbsSurfaceEvaluator::f(const std::string& attribute) const {
	NurbsSurfaceMesh* nsm = static_cast<NurbsSurfaceMesh*>(mesh);
	nfloat Energy = 0.0;
	for(size_t i = 0; i < nsm->SizeElements(); i++) {
		SNurbs* e = static_cast<SNurbs*>(nsm->GetElement(i));
		const NurbsPatchAttribute a(e, attribute);
		Energy += eval->f(a);
	}
	return Energy;
}

#ifdef _OPENMP
#include <omp.h>
#endif
Vector NurbsSurfaceEvaluator::Gradient(const std::string& attribute) const {
#ifdef _OPENMP
	NurbsSurfaceMesh* nsm = static_cast<NurbsSurfaceMesh*>(mesh);
	std::vector<Vector> ge(nsm->SizeElements(), ZeroVector(3 * nsm->GetElement(0)->SizeNodes()));
	
#pragma omp parallel for
	for(int i = 0; i < nsm->SizeElements(); i++) {
		SNurbs* e = static_cast<SNurbs*>(nsm->GetElement(i));
		const NurbsPatchAttribute a(e, attribute);
		ge[i] = eval->Gradient(a);
	}
	
	const size_t n = nsm->SizeNodes();
	Vector g = ZeroVector(3 * n);
	for(size_t i = 0; i < ge.size(); i++) {
		SNurbs* e = static_cast<SNurbs*>(nsm->GetElement(i));
		for(size_t j = 0; j < e->SizeNodes(); j++) {
			const size_t id = e->GetNode(j)->Id();
			g(3 * id + 0) += ge[i](3 * j + 0);
			g(3 * id + 1) += ge[i](3 * j + 1);
			g(3 * id + 2) += ge[i](3 * j + 2);
		}	
	}
#else
	NurbsSurfaceMesh* nsm = static_cast<NurbsSurfaceMesh*>(mesh);
	const size_t n = nsm->SizeNodes();
	Vector g = ZeroVector(3 * n);
	for(size_t i = 0; i < nsm->SizeElements(); i++) {
		SNurbs* e = static_cast<SNurbs*>(nsm->GetElement(i));
		const NurbsPatchAttribute a(e, attribute);
		const Vector gi = eval->Gradient(a);

		for(size_t j = 0; j < e->SizeNodes(); j++) {
			const size_t id = e->GetNode(j)->Id();
			g(3 * id + 0) += gi(3 * j + 0);
			g(3 * id + 1) += gi(3 * j + 1);
			g(3 * id + 2) += gi(3 * j + 2);
		}	
	}
#endif
	return g;
}

SparseMatrix NurbsSurfaceEvaluator::Hessian(const std::string& attribute) const {
	std::cout << "warning: Hessian not implemented." << std::endl;
	return SparseMatrix(0, 0, 0);
}
/////////////////////////////////////////////////////////////////////////////////////

nfloat EdgeEvaluator::f(const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	nfloat Energy = 0.0;
	for(size_t i = 0; i < tm->GetNumEdges(); i++) {
		SEdge* e = tm->Edge(i);
		if(e->neighbor && e->a->Id() < e->b->Id()) {
			std::pair< std::vector<size_t>, boost::tuples::tuple<boost::array<nfloat, 4>, boost::array<nfloat, 4>, boost::array<nfloat, 4> > > wedge = Wedge(e, attribute);
			const std::vector<size_t>& id = wedge.first;
			const boost::array<nfloat, 4>& x = boost::tuples::get<0>(wedge.second);
			const boost::array<nfloat, 4>& y = boost::tuples::get<1>(wedge.second);
			const boost::array<nfloat, 4>& z = boost::tuples::get<2>(wedge.second);
			const EdgeAttribute a(x, y, z);

			const nfloat energy = eval->f(a);

			e->a->Get<nfloat>("energy") += energy;
			e->b->Get<nfloat>("energy") += energy;

			Energy += energy;
		}
	}
	for(size_t k = 0; k < tm->GetNumVertices(); k++) {
		Node* v = tm->Vertex(k);
		nfloat& u = v->Get<nfloat>("energy");
		u = u / (nfloat)v->NumNeighbors();
	}
	return Energy;
}

nfloat EdgeEvaluator::f(Node* v, const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	std::vector<SEdge*> e;
	tm->GetRing(v, e);
	if(v->NumNeighbors() != e.size())
		std::cout << "???" << std::endl;

	nfloat Energy = 0.0;
	for(size_t i = 0; i < e.size(); i++) {
		std::pair< std::vector<size_t>, boost::tuples::tuple<boost::array<nfloat, 4>, boost::array<nfloat, 4>, boost::array<nfloat, 4> > > wedge = Wedge(e[i], attribute);

		const boost::array<nfloat, 4>& x = boost::tuples::get<0>(wedge.second);
		const boost::array<nfloat, 4>& y = boost::tuples::get<1>(wedge.second);
		const boost::array<nfloat, 4>& z = boost::tuples::get<2>(wedge.second);
		const EdgeAttribute a(x, y, z);

		Energy += eval->f(a);
	}

	return Energy;
}

Vector EdgeEvaluator::Gradient(const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	const size_t n = tm->SizeNodes();
	Vector Gradient = ZeroVector(3 * n);
	for(size_t i = 0; i < tm->GetNumEdges(); i++) {
		SEdge* e = tm->Edge(i);
		if(e->neighbor && e->a->Id() < e->b->Id()) {
			std::pair< std::vector<size_t>, boost::tuples::tuple<boost::array<nfloat, 4>, boost::array<nfloat, 4>, boost::array<nfloat, 4> > > wedge = Wedge(e, attribute);
			const std::vector<size_t>& id = wedge.first;
			const boost::array<nfloat, 4>& x = boost::tuples::get<0>(wedge.second);
			const boost::array<nfloat, 4>& y = boost::tuples::get<1>(wedge.second);
			const boost::array<nfloat, 4>& z = boost::tuples::get<2>(wedge.second);
			const EdgeAttribute ea(x, y, z);

			const Vector gradient = eval->Gradient(ea);
			GlobalAssembly(id, gradient, 4, Gradient);
		}
	}
	return Gradient;
}

SparseMatrix EdgeEvaluator::Hessian(const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	std::map< std::pair<size_t, size_t>, nfloat > entry;
	for(size_t i = 0; i < tm->GetNumEdges(); i++) {
		SEdge* e = tm->Edge(i);
		if(e->neighbor &&e->a->Id() < e->b->Id()) {
			std::pair< std::vector<size_t>, boost::tuples::tuple<boost::array<nfloat, 4>, boost::array<nfloat, 4>, boost::array<nfloat, 4> > > wedge = Wedge(e, attribute);
			const std::vector<size_t>& id = wedge.first;
			const boost::array<nfloat, 4>& x = boost::tuples::get<0>(wedge.second);
			const boost::array<nfloat, 4>& y = boost::tuples::get<1>(wedge.second);
			const boost::array<nfloat, 4>& z = boost::tuples::get<2>(wedge.second);
			const EdgeAttribute a(x, y, z);

			const Matrix hessian = eval->Hessian(a);
			GlobalAssembly(id, hessian, 4, entry);
		}
	}
	const size_t n = tm->SizeNodes();
	SparseMatrix Hessian(3 * n, 3 * n, entry.size());
	for(std::map< std::pair<size_t, size_t>, nfloat>::iterator it = entry.begin(); it != entry.end(); it++) {
 		const std::pair< std::pair<size_t, size_t>, nfloat>& p = *it;
 		Hessian(p.first.first, p.first.second) = p.second;
 	}
	return Hessian;
}

std::pair< std::vector<size_t>, boost::tuples::tuple<boost::array<nfloat, 4>, boost::array<nfloat, 4>, boost::array<nfloat, 4> > > EdgeEvaluator::Wedge(const SEdge* e, const std::string& attribute) const {
	const Node* v0 = e->a;
	const Node* v1 = e->b;

	std::vector<const Node*> vertex;
	for(size_t i = 0; i < 3; i++) {
		const Node* vi = e->triangle->vertex[i];
		const Node* vj = e->neighbor->triangle->vertex[i];
		if(vi != v0 && vi != v1) vertex.push_back(vi);
		if(vj != v0 && vj != v1) vertex.push_back(vj);
	}

	if(vertex.size() != 2) {
		std::cerr << "error: more than 2 vertices in list." << std::endl;
	}

	const Node* v2 = vertex[0];
	const Node* v3 = vertex[1];

	const Vec3n& x0 = v0->Get<Vec3n>(attribute); 
	const Vec3n& x1 = v1->Get<Vec3n>(attribute); 
	const Vec3n& x2 = v2->Get<Vec3n>(attribute); 
	const Vec3n& x3 = v3->Get<Vec3n>(attribute); 

	const boost::array<nfloat, 4> x = {x0[0], x1[0], x2[0], x3[0]};
	const boost::array<nfloat, 4> y = {x0[1], x1[1], x2[1], x3[1]};
	const boost::array<nfloat, 4> z = {x0[2], x1[2], x2[2], x3[2]};

	std::vector<size_t> id(4);
	id[0] = v0->Id();
	id[1] = v1->Id();
	id[2] = v2->Id();
	id[3] = v3->Id();

	return std::pair< std::vector<size_t>, boost::tuples::tuple<boost::array<nfloat, 4>, boost::array<nfloat, 4>, boost::array<nfloat, 4> > >(id, boost::tuples::tuple<boost::array<nfloat, 4>, boost::array<nfloat, 4>, boost::array<nfloat, 4> >(x, y, z));
}

/////////////////////////////////////////////////////////////////////////////////////
nfloat TriangleEvaluator::f(const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	nfloat Energy = 0.0;
	for(size_t i = 0; i < tm->GetNumTriangles(); i++) {
		STriangle* t = tm->Triangle(i);
		const TriangleAttribute a(t, attribute);

		const nfloat energy = eval->f(a);

		t->vertex[0]->Get<nfloat>("energy") += energy;
		t->vertex[1]->Get<nfloat>("energy") += energy;
		t->vertex[2]->Get<nfloat>("energy") += energy;

		Energy += energy;
	}
	for(size_t k = 0; k < tm->GetNumVertices(); k++) {
		Node* v = tm->Vertex(k);
		nfloat& u = v->Get<nfloat>("energy");
		u = u / (nfloat)v->NumNeighbors();
	}
	return Energy;
}

nfloat TriangleEvaluator::f(Node* v, const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	std::vector<STriangle*> ring = tm->GetRing(v);

	nfloat Energy = 0.0;
	for(size_t i = 0; i < ring.size(); i++) {
		STriangle* t = ring[i];
		const TriangleAttribute a(t, attribute);

		const nfloat energy = eval->f(a);

		Energy += eval->f(a);
	}

	return Energy;
}

Vector TriangleEvaluator::Gradient(const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	const size_t n = tm->SizeNodes();
	Vector g = ZeroVector(3 * n);
	for(size_t i = 0; i < tm->GetNumTriangles(); i++) {
		STriangle* t = tm->Triangle(i);
		const TriangleAttribute a(t, attribute);

		const Vector gi = eval->Gradient(a);

		for(size_t j = 0; j < 3; j++) {
			const size_t& id = t->vertex[j]->Id();
			g(3 * id) += gi(3 * j + 0); 
			g(3 * id + 1) += gi(3 * j + 1); 
			g(3 * id + 2) += gi(3 * j + 2); 
		}
	}
	return g;
}

SparseMatrix TriangleEvaluator::Hessian(const std::string& attribute) const {
	STriangleMesh* tm = static_cast<STriangleMesh*>(mesh);
	const size_t n = tm->SizeNodes();
	SparseMatrix h(3 * n, 3 * n, 0);
	return h;
}

/////////////////////////////////////////////////////////////////////////////////////
#ifdef _OPENMP
#include <omp.h>
#endif
nfloat Evaluator::f(const std::string& attribute) const {
	nfloat energy = 0.0;
#ifdef _OPENMP
#pragma omp parallel for
#endif
	for(int j = 0; j < mesh->SizeElements(); j++) {
		Element* e = mesh->GetElement(j);
		const ElementAttribute a(e, attribute);
		energy += eval->f(a);
	}

	return energy;
}

#ifdef _OPENMP
#include <omp.h>
#endif
Vector Evaluator::Gradient(const std::string& attribute) const {
	const size_t n = mesh->SizeNodes();
	const size_t ne = mesh->GetElement(0)->SizeNodes();
	Vector g = ZeroVector(3 * n);

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for(int j = 0; j < mesh->SizeElements(); j++) {
		Element* e = mesh->GetElement(j);
		const ElementAttribute a(e, attribute);
		const Vector gi = eval->Gradient(a);

		for(size_t i = 0; i < ne; i++) {
			const size_t& id = e->GetNode(i)->Id();
			g(3 * id) += gi(3 * i + 0); 
			g(3 * id + 1) += gi(3 * i + 1); 
			g(3 * id + 2) += gi(3 * i + 2); 
		}
	}

	return g;
}
/////////////////////////////////////////////////////////////////////////////////////
static void ApplyBoundaryCondition(SparseMatrix& A, std::vector<Vector>& b, const std::vector<size_t>& boundary) {
	typedef boost::numeric::ublas::matrix_row< SparseMatrix > Row;
	for(size_t k = 0; k < boundary.size(); k++) {
		const size_t& id = boundary[k];
		Row r(A, id);
		for(Row::iterator itr = r.begin(); itr != r.end(); itr++) {
			const size_t j = itr.index();
			A(id, j) = 0.0;
			A(j, id) = 0.0;
		}
		A(id, id) = 1.0;
		for(size_t j = 0; j < b.size(); j++)
			b[j](id) = 0.0;
	}
}

static Vector Solve3(SparseMatrix& A1, SparseMatrix& A2, const Vector& b, const std::vector<size_t>& boundary) {
	const size_t n = A1.size1();

	std::vector<Vector> B(3);
	B[0] = Vector(n);
	B[1] = Vector(n);
	B[2] = Vector(n);
	for(size_t i = 0; i < n; i++) {
		B[0](i) = b(3 * i);
		B[1](i) = b(3 * i + 1);
		B[2](i) = b(3 * i + 2);
	}

	for(size_t i = 0; i < 3; i++)
		B[i] = prod(A2, B[i]);

	ApplyBoundaryCondition(A1, B, boundary);
	
	std::vector<Vector> x(3);
	x[0] = Vector(n);
	x[1] = Vector(n);
	x[2] = Vector(n);

#ifdef __MKL__
	SolvePardiso(A1, B, x, false);
#else
	Time::Start();
	for(size_t i = 0; i < 3; i++) {
		SUPERLU_wrapper::Solve(A1, B[i], x[i]);
	}
	Time::Stop();
#endif
	
	Vector X(3 * n);
	for(size_t i = 0; i < n; i++) {
		X(3 * i) = x[0](i);
		X(3 * i + 1) = x[1](i);
		X(3 * i + 2) = x[2](i);
	}

	return X;
}

Vector SobolevGradientVariableFilterPreconditioner::operator()(const Vector& g) const {
	typedef boost::numeric::ublas::matrix_row< SparseMatrix > Row;
	const size_t n = mesh->SizeNodes();
	const size_t korder = 2;

	SparseMatrix M = MassMatrix(*mesh, attribute, false, true);
	
	SparseMatrix Minv = SparseMatrix(n, n, n);
	for(size_t i = 0; i < n; i++) {
		const nfloat mii = M(i, i);
		Minv(i, i) = 1.0 / M(i, i);
	}
	
	SparseMatrix K = StiffnessMatrix(*mesh, attribute, false);
// 	SparseMatrix K = 1.0 * CotangentWeights(*tm, attribute);
	
	std::vector<SparseMatrix> L(korder + 1);
	L[0] = SparseMatrix(n, n, n);
	for(size_t i = 0; i < n; i++)
		L[0](i, i) = 1.0;

	for(size_t i = 1; i <= korder; i++)
		L[i] = SparseMatrix(n, n, 0);
	
	boost::numeric::ublas::sparse_prod(Minv, K, L[1]);

	for(size_t i = 2; i <= korder; i++) {
		boost::numeric::ublas::sparse_prod(L[i - 1], L[1], L[i]);
	}
	
	std::vector<nfloat> a(korder + 1, 0.0);
	std::vector<nfloat> b(korder + 1, 0.0);

	switch(order) {
		case 0:
			a[0] = 1.0;
			a[1] = 1.0;
			a[2] = 0.0;

			b[0] = 0.0;
			b[1] = 1.0;
			b[2] = 0.0;
			break;
		case 1:
			a[0] = 1.0;
			a[1] = 0.0;
			a[2] = 1.0;

			b[0] = 0.0;
			b[1] = 2.0;
			b[2] = 0.0;
			break;
		case 2:
			a[0] = 1.0;
			a[1] = 0.0;
			a[2] = 1.0;

			b[0] = 1.0;
			b[1] = 0.0;
			b[2] = 0.0;
			break;
	}
	
	SparseMatrix La = L[0];
	SparseMatrix Lb = L[0];
	for(size_t i = korder - 1; i <= korder; i++) {
		if((i % 2) != 0) {
			if(a[i] > 0.0001) La -= a[i] * L[i];
			if(b[i] > 0.0001) Lb -= b[i] * L[i];
		} else {
			if(a[i] > 0.0001) La += a[i] * L[i];
			if(b[i] > 0.0001) Lb += b[i] * L[i];
		}
	}

	SparseMatrix La2(n, n, 0);
	SparseMatrix Lb2(n, n, 0);
	boost::numeric::ublas::sparse_prod(M, La, La2);
	boost::numeric::ublas::sparse_prod(M, Lb, Lb2);

	Vector g2 = g;
	for(size_t i = 0; i < n; i++) {
		const nfloat mii = Minv(i, i);
		g2(3 * i + 0) *= mii; 
		g2(3 * i + 1) *= mii; 
		g2(3 * i + 2) *= mii; 
	}
	const Vector s = Solve3(La2, Lb2, g2, boundary);
	currentStep++;

	return s;
}
			
void SobolevGradientPreconditioner::Update() {
	std::cout << "computing mass Matrix..." << std::endl;
	const size_t n = mesh->SizeNodes();
	M = MassMatrix(*mesh, attribute, false, true);
	Minv = SparseMatrix(n, n, n);
	for(size_t i = 0; i < n; i++) {
		const nfloat mii = M(i, i);
		Minv(i, i) = 1.0 / M(i, i);
	}

	if(order > 0) {
		std::cout << "computing stiffness Matrix..." << std::endl;
		K = StiffnessMatrix(*mesh, attribute, false);
// 		K = 1.0 * CotangentWeights(*tm, attribute);
	}
}

Vector SobolevGradientPreconditioner::operator()(const Vector& g) const {
	bool flag = true;
	typedef boost::numeric::ublas::matrix_row< SparseMatrix > Row;
	const size_t n = mesh->SizeNodes();

	Vector g2 = g;
	for(size_t i = 0; i < n; i++) {
		const nfloat mii = Minv(i, i);
		g2(3 * i + 0) *= mii; 
		g2(3 * i + 1) *= mii; 
		g2(3 * i + 2) *= mii; 
	}

	if(order == 0)
		return g2;
	
	std::vector<SparseMatrix> L(order + 1);
	L[0] = SparseMatrix(n, n, n);
	for(size_t i = 0; i < n; i++)
		L[0](i, i) = 1.0;

	for(size_t i = 1; i <= order; i++)
		L[i] = SparseMatrix(n, n, 0);
	
	boost::numeric::ublas::sparse_prod(Minv, K, L[1]);

	for(size_t i = 2; i <= order; i++) {
		boost::numeric::ublas::sparse_prod(L[i - 1], L[1], L[i]);
	}
	
	nfloat sign = 1.0;
	if((order % 2) != 0)
		sign = -1.0;
	else
		sign = 1.0;

	for(size_t i = 0; i < L[order].size1(); i++) {
		Row rA(L[order], i); 
		for(Row::iterator itr = rA.begin(); itr != rA.end(); itr++) {
			const size_t j = itr.index();
			nfloat a = *itr;
			if(i == j)
				*itr = 1.0 + sign * a;
			else
				*itr = sign * a;
		}
	}

	std::cout << "solving linear system..." << std::endl;
	const Vector s = Solve3(L[order], L[0], g2, boundary);//solves L^order * s = Minv g

	return s;
}

std::vector< std::pair<nfloat, nfloat> > Minimize1(Evaluator* eval, GradientPreconditioner* gpc, StepSizeEstimator* sse, const std::string& attribute, const nfloat eps, const nfloat epsChangeOrder, const size_t maxIterations) {
	Mesh* mesh = eval->GetMesh();
	const size_t n = mesh->SizeNodes();
	
	const size_t m = 3;
	const size_t k = 10;
	std::list<Vec2n> sample;
	nfloat t = 0.0;

	Time::Start();
	Vector g0 = eval->Gradient(attribute);
	nfloat E0 = eval->f(attribute);
	PRINTVAR(E0);

	gpc->Update();
	Vector G0 = (*gpc)(g0);

	Vector S0 = -1.0 * G0;

	nfloat eps2 = eps;
	std::vector< std::pair<nfloat, nfloat> > r;
	std::vector<nfloat> slope;
	size_t countRestart = 0;
	for(size_t i = 0; i < maxIterations; i++) {
		std::cout << "linesearch..." << std::endl;
		std::pair<nfloat, nfloat> p = (*sse)(eps2, S0);
		std::cout << "done." << std::endl;

		if(p.second > E0) {
			S0 = -1.0 * G0;
			p = (*sse)(eps2, S0);
			if(p.second > E0) {
				eps2 *= 0.5;
				std::cout << "warning: energy increases, reducing epsilon to " << eps2 << std::endl;
				
				if(countRestart > 6)
					break;
				else
					continue;
			}
		}
		
		const nfloat& alpha = p.first;
		const nfloat& E1 = p.second;

		sample.push_back(math::V2n(static_cast<nfloat>(i), E1));
		if(sample.size() > m)
			sample.pop_front();

		if(sample.size() >= m) {
			std::pair<nfloat, nfloat> fit = GSL_wrapper::LinearFit(sample);
			const nfloat s = std::abs(fit.second);
			if(!slope.empty() && s < epsChangeOrder * slope[0]) {
				std::cout << "warning: slow convergence." << std::endl;
				return r;
			}
			slope.push_back(s);
		}

		nfloat maxOffset = 0.0;
		for(size_t j = 0; j < n; j++) {
			Node* v = static_cast<Node*>(mesh->GetNode(j));
			if(v->Get<size_t>("mark") == 0) {
				const size_t& id = v->Id();
				Vec3n& P = v->Get<Vec3n>(attribute);

				const Vec3n localGradient = math::V3n(S0(3 * id + 0), S0(3 * id + 1), S0(3 * id + 2));
				
				const Vec3n offset = alpha * localGradient;

				P += offset;

				const nfloat d = length(offset);
				if(d > maxOffset)
					maxOffset = d;
			} 
		}
		Time::Stop();
// 		t += Time::GetTime();
		t += alpha;
		r.push_back(std::pair<nfloat, nfloat>(t, E1));
		PRINTVAR4(i, alpha, E1, maxOffset);
		Time::Start();

		if(i < maxIterations - 1) {
			eval->Initialize(attribute);
			const Vector g1 = eval->Gradient(attribute);

			gpc->Update();
			Vector G1 = (*gpc)(g1);

			const nfloat beta = inner_prod(G1, G1) / inner_prod(G0, G0);
			// 		const nfloat beta = inner_prod(G1 - G0, G1) / inner_prod(G0, G0);
			S0 = beta * S0 - G1;
// 			S0 = -G1;

			G0 = G1;
			E0 = E1;
		}
	}

	return r;
}

std::vector< std::pair<nfloat, nfloat> > MinimizeCG(Evaluator* eval, GradientPreconditioner* gpc, StepSizeEstimator* sse, const std::string& attribute, const nfloat eps, const nfloat epsChangeOrder, const size_t maxIterations) {
	return Minimize1(eval, gpc, sse, attribute, eps, epsChangeOrder, maxIterations);
}

void MinimizeMultiScale(Mesh* mesh, Evaluator* eval, const Config& config) {
	if(!mesh->ExistNodeAttribute("positionTemp"))
		mesh->AddNodeAttribute<Vec3n>("positionTemp", math::V3n(0.0, 0.0, 0.0));

	if(!eval->GetMesh()->ExistNodeAttribute("energy"))
		eval->GetMesh()->AddNodeAttribute<nfloat>("energy", 0.0);

	const nfloat& eps = config.eps;
	const nfloat& epsChangeOrder = config.epsChangeOrder;
	const size_t& maxIterations = config.maxIterations;
	const size_t& maxLoops = config.maxLoops;
	const size_t& minOrder = config.minOrder;
	const size_t& maxOrder = config.maxOrder;

	Optimizer::GradientPreconditioner* gpc;
	if(config.flagPrecondition)
		gpc = new Optimizer::SobolevGradientPreconditioner(mesh, "position", maxOrder);
	else
		gpc = new Optimizer::GradientPreconditioner();

	Optimizer::StepSizeEstimator* sse;
	if(config.flagLineSearch)
		sse = new Optimizer::LineSearchStepSizeEstimator(mesh, eval, "position", "positionTemp");
	else
		sse = new Optimizer::StepSizeEstimator(mesh, eval, "position", "positionTemp");

	std::string path = GeneratePath("output_");
	
	std::vector<size_t> korder;
	std::vector<bool> direction;

	if(minOrder == maxOrder) {
		for(size_t i = 0; i < maxLoops; i++) {
			korder.push_back(maxOrder);
			direction.push_back(true);
		}
	} else {
		for(size_t i = 0; i < maxLoops; i++) {
			for(size_t j = minOrder; j < maxOrder; j++) {
				direction.push_back(true);
				korder.push_back(j);
			}
			for(size_t j = minOrder; j < maxOrder; j++) {
				direction.push_back(false);
				korder.push_back(maxOrder - j + minOrder);
			}
		}
		korder.push_back(minOrder);
		direction.push_back(false);
	}

	for(size_t i = 0; i < korder.size(); i++)
		PRINTVAR2(korder[i], direction[i]);

	std::stringstream ss;
	ss << GetPath(path);
	std::string datapath = ss.str();
	std::cout << "creating directory \"" << datapath << "\"..." << std::endl;
	boost::filesystem::create_directory(datapath);

	std::string filenameInfo = datapath + "/config.xml";
	config.Save(filenameInfo);

	std::cout << std::endl;
	
	std::vector<Vec2n> p;
	p.push_back(math::V2n(0.0, eval->f("position")));
	for(size_t i = 0; i < korder.size(); i++) {
		PRINTVAR2(korder[i], direction[i]);
		gpc = new Optimizer::SobolevGradientPreconditioner(mesh, "position", korder[i], maxIterations, direction[i]);

		const std::vector< std::pair<nfloat, nfloat> > E = Optimizer::MinimizeCG(eval, gpc, sse, "position", eps, epsChangeOrder, maxIterations);

		std::vector<Vec2n> pi;
		pi.push_back(p.back());

		nfloat t = p.back()[0];
		for(size_t j = 0; j < E.size(); j++) p.push_back(math::V2n((nfloat)(t + E[j].first), E[j].second));
		for(size_t j = 0; j < E.size(); j++) pi.push_back(math::V2n((nfloat)(t + E[j].first), E[j].second));

		std::stringstream ssIndex;
		ssIndex << std::setw(4) << std::setfill('0') << i << "_" << std::setw(4) << std::setfill('0') << korder[i];

		std::stringstream ss1;
		ss1 << datapath << "/energy_" << ssIndex.str() << ".txt";
		SavePoints<2>(pi, ss1.str());
		
		std::stringstream ss2;
		ss2 << datapath << "/mesh_" << ssIndex.str() << ".txt";
		PRINTVAR(ss2.str());
		mesh->Save(ss2.str(), "position");
	}
}

}
